#include <cstdio> // printf

#include "Spring.h"

Spring::Spring(Particle* _particle1, Particle* _particle2, double _stiffness) :
	particle1(_particle1), particle2(_particle2), stiffness(_stiffness)
{
	restLength = (particle1->position - particle2->position).length();
}

void Spring::satisfyConstraint()
{
	// TODO: approximate sqrt with Taylor expansion for increased speed
	// see: http://www.gotoandplay.it/_articles/2005/08/advCharPhysics.php

	double im1 = particle1->invMass;
	double im2 = particle2->invMass;
	// Handle special case
	if(im1+im2 == 0)
		return;
	// Direction vector
	Vector3 dir = (particle2->position - particle1->position);
	double curLength = dir.length();
	if(curLength == 0)
		return;
	//fprintf(stderr, "Spring::satisfyConstraint(): %.3lf %.3lf\n", restLength, curLength);
	// Scalar factor
	double scalar = (curLength-restLength)/(curLength*(im1+im2))*stiffness;
	particle1->position += im1*dir*scalar;
	particle2->position -= im2*dir*scalar;
}
